//
// Created by xujingyi on 2021/4/28.
//
#include <algorithm>
#include <string>
#include <map>
#include "armor_finder.h"
#include "solve/Predictor.h"
#include "solve/SmoothKF.h"
//#include <opencv2/core.hpp>
//#include <opencv2/highgui.hpp>
#include "Setter.h"
#include "log.h"
#include "additions.h"
#include "show_images.h"
#include "Serial.h"

extern rm::Setter rm_setter;
extern cv::Mat origin,show_pic,pre_pic;
extern rm::Predictor predictor;
extern rm::GyroPredictor gyro_pretr;
extern rm::Smoother smoother;

int anti_switch_cnt=0;//尽量寻找同一目标的尝试次数
static int tracking_time_cnt=0;//保持tracking模式最长多少帧
static int spinning_time_cnt=0;//保持spinning模式最长多少帧，tracking模式下就开始计数，积累到一定数值以后切入spinning模式
int lost_then_keep_tracking_cnt=0;//当丢失目标一定帧数内暂时不切换入searching模式
static bool just_into_search=true;//用于标志进入search模式的第一帧，切入前置为true，结束置false

/*!
 * 根据当前找到的装甲板位置设置下一帧的ROI区域
 * @return cv::Rect2f _track_box
 */
cv::Rect2f rm::ArmorFinder::_setNextTrackBox(const rm::Armor &armor){
    float height_coeff=1.0,width_coeff=1.0,h_plus=0,w_plus=0;
    if(abs(serial.rxpackage.pitch_speed.type_short)<rm_setter.cruise_judge_speed/2.0) height_coeff*=0.4;
    else h_plus=(armor.box_pos.getPNPCen3d().z-3000)/1000.0f;
    if(abs(serial.rxpackage.yaw_speed.type_short)<rm_setter.cruise_judge_speed) width_coeff*=0.5;
    else w_plus=(armor.box_pos.getPNPCen3d().z-3000)/1000.0f;
    h_plus=h_plus>0?h_plus:0;
    w_plus=w_plus>0?w_plus:0;
//    cv::Rect last_bounding_box=cv::boundingRect(last_armor.vertexes());
    cv::Rect2f last_bounding_box=cv::Rect2f(armor.couple.getNumBox());
    float width_ratio=(width_coeff*rm_setter.tracking_box_plus_ratio)*last_bounding_box.width+w_plus;
    float height_ratio=(height_coeff*rm_setter.tracking_box_plus_ratio)*last_bounding_box.height+h_plus;
    _track_box=last_bounding_box-cv::Point2f(0.5*width_ratio,0.5*height_ratio);
    _track_box=_track_box+cv::Size2f(width_ratio,height_ratio);
    if(armor.getID()==1||armor.getID()==8) {
        _track_box=cv::Rect2f(_track_box)-cv::Point2f(_track_box.width/2.0f,0)+cv::Size2f(_track_box.width,0);
    }
    _track_box=_track_box&cv::Rect2f(0,0,PICTURE_COL_NUM,PICTURE_ROW_NUM);
    return _track_box;
};

/*!
 * 设置下一次的陀螺ROI区域
 * @return
 */
cv::Rect2f rm::ArmorFinder::_setNextGyroBox(const rm::Armors &armors){
    if(armors.size()>1){
        cv::circle(show_pic,_setNextTrackBox(gyro_tool.left_armor).tl(),2,COLOR_GREEN,-1);
        cv::circle(show_pic,_setNextTrackBox(gyro_tool.right_armor).br(),2,COLOR_GREEN,-1);
        _track_box=cv::Rect2f(_setNextTrackBox(gyro_tool.left_armor).tl(),_setNextTrackBox(gyro_tool.right_armor).br());
    }
    _track_box=_track_box&cv::Rect2f(0,0,PICTURE_COL_NUM,PICTURE_ROW_NUM);
    return _track_box;
};

extern int deal_buffer;
/*!
 * 搜索目标
 * 在全图/ROI区域内搜索灯条、匹配灯条对、数字识别、决策装甲版
 * @param img2show 用于显示的图片-
 * @return
 */
rm::Armors rm::ArmorFinder::searchingTarget(const cv::Mat &img2show,const cv::Point2f offset){
#ifdef DEBUG
    std::cout<<"4:dealing[5]:"<<deal_buffer<<std::endl;
#endif
    if (img2show.empty()) return rm::Armors();
    cv::Mat track_origin=img2show.clone();
#ifdef DEBUG
    std::cout<<"4:dealing[6]:"<<deal_buffer<<std::endl;
#endif
    rm::MarkerCouples cps=matchMarkerCouple(findMarker(preProcess(track_origin),img2show) ,img2show);
#ifdef DEBUG
    std::cout<<"4:dealing[7]:"<<deal_buffer<<std::endl;
#endif
    if(cps.size()<=0) return rm::Armors();
#ifdef DEBUG
    std::cout<<"4:dealing[8]:"<<deal_buffer<<std::endl;
#endif
    return findArmor(cps,img2show,track_origin,offset);
};

/*!
 * Searching模式主函数
 * @param src
 * @return 是否搜索到目标（搜索到：true）
 */
bool rm::ArmorFinder::stateSearchingTarget(cv::Mat &src){
    _armors=rm::ArmorFinder::searchingTarget(src,cv::Point2f (0,0));
    target_armor=decision(_armors,src);
#ifdef DEBUG
    std::cout<<"4:dealing[9]:"<<deal_buffer<<std::endl;
#endif
    if(target_armor.getID()<=0) {
        target_armor=Armor();
        setFindLastTargetFlag(0);
        return false;
    }
    else{
//        _setNextTrackBox();
        setFindLastTargetFlag(1);
//        for(rm::Armor &armor:_armors) {
//            switch (_spinning_state_flag) {
//                case true: if(armor.getID()==gyro_tool.getID()) gyro_armors.emplace_back(armor); break;
//                case false: if(armor.getID()==last_track_armor.getID()) gyro_armors.emplace_back(armor); break;
//            }
//        }

        pre4Gyro(_armors,_track_box.tl());
#if (SOLVE_MODE==PIXEL_PREDICT_MODE)||(SOLVE_MODE==ANTI_PREDICT_MODE)
        cv::Point3f pos_message=pre4Normal();
#elif SOLVE_MODE==PIXEL_ANGLE_MODE
        cv::Point3f pos_message=posResolve(target_armor,cv::Point2f(0,0));
#endif
        target_armor.box_pos.setPosMessage(pos_message);
        showResult(show_pic, target_armor,pos_message);
        return true;
    }
};

extern int gSrcNum;
extern std::string Num_Path1;
/*!
 * Tracking模式主函数
 * @param src
 * @return 是否搜索到目标（搜索到：true）
 */
bool rm::ArmorFinder::stateTrackingTarget(cv::Mat &src){
#ifdef TEST_THREAD
    std::cout<<"4:dealing[4]:"<<deal_buffer<<std::endl;
#endif
    if(_track_box==cv::Rect2f()) {
        LOGE("TRACK_BOX IS EMPTY");
        return false;
    }
    cv::Mat roi=src(_track_box);
    _armors=rm::ArmorFinder::searchingTarget(roi,_track_box.tl());
    target_armor=decision(_armors,roi);
#ifdef DEBUG
    std::cout<<"4:dealing[9]:"<<deal_buffer<<std::endl;
#endif
    if(target_armor.getID()<=0) {
        target_armor=Armor();
//    setFindLastTargetFlag(0);
        return false;
    }
    else{
        setFindLastTargetFlag(1);
//        rm::Armors gyro_armors;
//        for(rm::Armor &armor:_armors) {
//            switch (_spinning_state_flag) {
//                case true: if(armor.getID()==gyro_tool.getID()) gyro_armors.emplace_back(armor); break;
//                case false: if(armor.getID()==last_track_armor.getID()) gyro_armors.emplace_back(armor); break;
//            }
//        }

        pre4Gyro(_armors,_track_box.tl());
#if (SOLVE_MODE==PIXEL_PREDICT_MODE)||(SOLVE_MODE==ANTI_PREDICT_MODE)
        cv::Point3f pos_message=pre4Normal();
#elif SOLVE_MODE==PIXEL_ANGLE_MODE
        cv::Point3f pos_message=posResolve(target_armor,_track_box.tl());
#endif

        target_armor.box_pos.setPosMessage(pos_message);
        showResult(show_pic, target_armor,pos_message);
        if(!_spinning_state_flag) _setNextTrackBox(target_armor);

//        roi.copyTo(show_pic);
        return true;
    }
};

/*!
 * 判断陀螺
 * @return
 */
bool rm::ArmorFinder::judgeSpinning(){
#ifdef GYRO_MODE
//    std::cout<<gyro_tool.qEulars[-1]<<" "<<gyro_tool.qEulars.absDiffMean()*gyro_tool.qEulars.length()/gyro_tool.qEulars.size()<<" "<<abs(gyro_tool.qOmegas.mean())*gyro_tool.qOmegas.length()/gyro_tool.qOmegas.size()<<std::endl;
    if(gyro_tool.qEulars.absDiffMean()*gyro_tool.qEulars.length()/gyro_tool.qEulars.size()>rm_setter.switch2gyro_absdif_limit&&abs(gyro_tool.qOmegas.mean())*gyro_tool.qOmegas.length()/gyro_tool.qOmegas.size()>rm_setter.switch2gyro_velocity_limit)
        return true;
#endif
    return false;
}

bool rm::ArmorFinder::stateSpinningTarget(cv::Mat &src){
    if(_track_box==cv::Rect2f()) {
        LOGE("TRACK_BOX IS EMPTY");
        return false;
    }
    cv::Mat roi=src(_track_box);
    _armors=rm::ArmorFinder::searchingTarget(roi,_track_box.tl());
    if(_armors.empty()) return false;

    rm::Armors gyro_armors;
    for(rm::Armor &armor:_armors) if(armor.getID()==gyro_tool.getID()) gyro_armors.emplace_back(armor);

    pre4Gyro(gyro_armors,_track_box.tl());
#ifdef GYRO_MODE==PART_MODE
    target_armor=gyrodecision(gyro_armors,roi);
    gyro_tool.setPosMessage(pre4Normal());
#endif

    _setNextGyroBox(gyro_armors);
    return true;
};

/*!
 * 计算相对于另一个发射机构世界坐标系的pitch角度以及yaw角度
 * @param world_coord 相对于当前发射机构坐标系的世界坐标
 * @return 相对于另一个发射机构世界坐标系的pitch角度(向下为正)以及yaw角度(?)
 */
cv::Point2f calcuAnotherPYAngle(const cv::Point3f &world_coord){
    if(world_coord==cv::Point3f()) return cv::Point2f();
    cv::Point3f another_world_coord;
#if SHOOTER_MODE==UP_SHOOTER
    another_world_coord+=cv::Point3f(0,0,DOWN_RAIL_DIFF-UP_RAIL_DIFF);
#else SHOOTER_MODE==DOWN_SHOOTER
    another_world_coord+=cv::Point3f(0,0,UP_RAIL_DIFF-DOWN_RAIL_DIFF);
#endif
    return cv::Point2f(tan(another_world_coord.y/another_world_coord.z)*ANGLE_PER_RAD,tan(another_world_coord.x/another_world_coord.z)*ANGLE_PER_RAD);
}

/*!
 * 寻找目标函数，实现各种模式的切换逻辑
 * @param src 相机采集到的裁减后的图像
 */
void rm::ArmorFinder::findTarget(cv::Mat &src){
#ifdef TEST_DEBUG
    std::cout<<"4:dealing[2]:"<<deal_buffer<<std::endl;
#endif
    if(!src.empty()) {
        origin=src.clone();
        show_pic=src.clone();
        last_armor=target_armor;
        bool is_not_so_far=bool(getPointLength(last_track_armor.center(),last_armor.center())<1*cv::boundingRect(last_track_armor.vertexes()).height);
        bool is_same_id = bool((last_track_armor.getID() == last_armor.getID())&&last_track_armor.getID()!=-1);
//        bool is_same_box = bool(is_same_id&&);
#ifdef TEST_DEBUG
        std::cout<<"4:dealing[3]:"<<deal_buffer<<std::endl;
#endif
        if(rm_setter.print_state_switch_test_info){
            LOGM("tracking_cnt:%d, lose_keep:%d, getFindLastTargetFlag:%d, is_same_id:%d, is_not_so_far:%d, last_track_armor_id:%d, last_target_id:%d, last_target_armor<last_track_armor:%d",tracking_time_cnt,lost_then_keep_tracking_cnt,int(getFindLastTargetFlag()),is_same_id,is_not_so_far,last_track_armor.getID(),last_armor.getID(),int(last_armor<last_track_armor));
//        std::cout<<"last-track_center:"<<getPointLength(last_track_armor.center(),target_armor.center())<<"  target:"<<cv::boundingRect(last_track_armor.vertexes()).height<<std::endl;
        }
        _calcRotMatrix();
        bool _track_find_flag=false;
        switch (state) {
            case SEARCHING_STATE:
                if(rm_setter.print_state_switch_test_info) std::cout<<"[SEARCH]:_track_box.tl()"<<_track_box.tl()<<" "<<_track_box.size()<<std::endl;
                if (stateSearchingTarget(show_pic)) {
                    lost_then_keep_tracking_cnt=0;
//                    LOGM("is_same_id:%d,is_not_so_far:%d",is_same_id,is_not_so_far);
//                    bool cond = bool((getFindLastTargetFlag() == true) && (!is_same_id )&&(target_armor.getID()!=1))||is_so_far;
                    bool try_to_find_last_target_fail=(((getFindLastTargetFlag()==true)&&(anti_switch_cnt++ > rm_setter.anti_switch_cnt_limit) && (!is_same_id)))||(last_track_armor.getID()==-1);
                    if(try_to_find_last_target_fail||(is_same_id&&is_not_so_far)||(!is_same_id&&last_armor<last_track_armor)){//id4>id1
//                        std::cout<<try_to_find_last_target_fail<<" "<<(is_same_id&&is_not_so_far)<<" "<<(!is_same_id&&last_armor<last_track_armor)<<std::endl;
                        anti_switch_cnt = 0;
                        //if ((target_armor.rect & cv::Rect2d(0, 0, PICTURE_COL_NUM, PICTURE_ROW_NUM)) == target_armor.rect) { // 判断装甲板区域是否脱离图像区域

                        //tracker = TrackerToUse::create();                       // 成功搜寻到装甲板，创建tracker对象
                        //tracker->init(src, target_box.rect);
                        _setNextTrackBox(target_armor);
                        last_state=state;
                        state = TRACKING_STATE;
                        last_track_armor=target_armor;
                        last_armor=target_armor;
                        if(try_to_find_last_target_fail||(!is_same_id&&last_armor<last_track_armor)) {
                            predictor.Boxposes.clear();
                            LOGM(STR_CTR(WORD_LIGHT_BLUE, "Change Target to %d"),target_armor.getID());
                        }
                        if(_spinning_state_flag&&!(is_same_id&&is_not_so_far)) {
                            gyro_tool.exit();//just exit from gyro state cause of lost target
                            _spinning_state_flag=false;
                        }
                        tracking_time_cnt = 0;
                        LOGM(STR_CTR(WORD_LIGHT_CYAN, "into TRACK"));
                        just_into_search=false;
                        break;
                    }
                    setFindLastTargetFlag(true);
                }
                just_into_search=false;
                break;
            case TRACKING_STATE:
                if(rm_setter.print_state_switch_test_info) std::cout<<"[TRACK]:_track_box.tl()"<<_track_box.tl()<<" "<<_track_box.size()<<std::endl;
                tracking_time_cnt++;
                _track_find_flag=stateTrackingTarget(show_pic);
                if(tracking_time_cnt>rm_setter.tracking_time_cnt_limit){
                    anti_switch_cnt = 0;
                    setFindLastTargetFlag(false);
                    lost_then_keep_tracking_cnt=0;
                    tracking_time_cnt=0;
                    last_state=state;
                    state=SEARCHING_STATE;
                    _track_box=cv::Rect2f();
//                    predictor.init();
//                    smoothkf.initSmoothKF();
                    LOGM(STR_CTR(WORD_LIGHT_PURPLE, "into SEARCH"));
                    just_into_search=true;
                    if(_track_find_flag){
                        last_track_armor=target_armor;
                        last_armor=target_armor;
                    }
                    else{
                        target_armor=last_armor;
                        last_track_armor=last_armor;
                    }
                    break;
                }
                if(!_track_find_flag||(last_armor.getID() != target_armor.getID())){
                    if(lost_then_keep_tracking_cnt++>rm_setter.lost_then_keep_tracking_cnt_limit){
                        setFindLastTargetFlag(false);
                        lost_then_keep_tracking_cnt=0;
                        tracking_time_cnt=0;
//                        target_armor=last_armor;
//                        target_armor.box_pos.setPosMessage(cv::Point3f(0,0,0));
                        last_state=state;
                        state=SEARCHING_STATE;
//                        predictor.Boxposes.clear();
                        smoother.init();
                        LOGM(STR_CTR(WORD_LIGHT_PURPLE, "into SEARCH(init)"));
                        just_into_search=true;
                        last_track_armor=rm::Armor();
                        _track_box=cv::Rect2f();
                        break;
                    }
                    else {
                        setFindLastTargetFlag(true);
                        target_armor=last_armor;
                        _track_box=_track_box-cv::Point2f(0.5*_track_box.width,0.5*_track_box.height);
                        _track_box=_track_box+cv::Size2f(1*_track_box.width,1*_track_box.height);
                        _track_box=_track_box&cv::Rect2f(0,0,PICTURE_COL_NUM,PICTURE_ROW_NUM);
//                        LOGE("target_armor=last_armor:message:%lf", target_armor.box_pos.getPosMessage().z);
                    }
                }
                else{
                    lost_then_keep_tracking_cnt=0;
                    setFindLastTargetFlag(true);
                    last_track_armor=target_armor;
                    if(judgeSpinning()) {
                        if(last_state!=GYRO_STATE){//NOT from gyro state cause of spinning_time_cnt out time
                            _track_box=_track_box-cv::Point2f(_track_box.width,_track_box.height);
                            _track_box=_track_box+cv::Size2f(2*_track_box.width,2*_track_box.height);
                            _track_box=_track_box&cv::Rect2f(0,0,PICTURE_COL_NUM,PICTURE_ROW_NUM);
                            gyro_tool.enter(last_armor.getID());
                            LOGM(STR_CTR(WORD_LIGHT_BLUE, "into gyro"));
                        }
                        else LOGM(STR_CTR(WORD_LIGHT_BLUE, "reinto gyro"));
                        last_state=state;
                        state=GYRO_STATE;
                        break;
                    }
                    else {
                        if(_spinning_state_flag) gyro_tool.exit();//just exit from gyro state cause of lost target
                        _spinning_state_flag=false;
                    }
                }

                break;
            case GYRO_STATE:
                if(rm_setter.print_state_switch_test_info) std::cout<<"[GYRO]:_track_box.tl()"<<_track_box.tl()<<" "<<_track_box.size()<<std::endl;
                _spinning_state_flag=true;
                tracking_time_cnt++;
                if(spinning_time_cnt++>rm_setter.spinning_time_cnt_limit){//if find same target but in gyro_state so long time
                    spinning_time_cnt=0;
                    last_state=state;
                    state=TRACKING_STATE;
                    LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
                }
                if(stateSpinningTarget(src)) {//if find same target and not in gyro_state so long time
                    lost_then_keep_tracking_cnt=0;
                    setFindLastTargetFlag(true);
                }
                else {
                    if(lost_then_keep_tracking_cnt++>rm_setter.lost_then_keep_tracking_cnt_limit){
                        setFindLastTargetFlag(false);
                        spinning_time_cnt=0;
                        last_state=state;
                        state=SEARCHING_STATE;
                        LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!"));
                        just_into_search=true;
                        last_track_armor=target_armor;
                        gyro_tool.exit();
                        gyro_pretr.init();
                    }
                    else {
                        _track_box=_track_box-cv::Point2f(_track_box.width,_track_box.height);
                        _track_box=_track_box+cv::Size2f(2*_track_box.width,2*_track_box.height);
                        _track_box=_track_box&cv::Rect2f(0,0,PICTURE_COL_NUM,PICTURE_ROW_NUM);
                        target_armor=last_armor;
                    }
                }
        }
        Txpackage txp;
        cv::Point3f send_message=cv::Point3f(0,0,0);
        cv::Point3f world_coord=cv::Point3f(0,0,0);
        if(_spinning_state_flag) {
            send_message=gyro_tool.getPosMessage();
            world_coord=gyro_tool.gyro_center;
        }
        else {
            send_message=target_armor.box_pos.getPosMessage();
            world_coord=target_armor.box_pos.getWorldCoord();
        }
        if(abs(send_message.x)>40||abs(send_message.y)>50){
            predictor.init();
            gyro_pretr.init();
            send_message=cv::Point3f(0,0,0);
            LOGE("Predictor wrong!");
        };
        cv::Point2f another_py=calcuAnotherPYAngle(world_coord);
        if(!just_into_search&&send_message==cv::Point3f()) {
            setShootflag(false);
            txp(cv::Point3f(0,0,0),0,0,getShootflag());
        }
        else {
            setShootflag(true);
#if SHOOTER_MODE == UP_SHOOTER
            if((target_armor.box_pos.getPosMessage().z<rm_setter.up_min_shoot_distance)||(target_armor.box_pos.getPosMessage().z>rm_setter.up_max_shoot_distance)) setShootflag(false);
#else
#ifndef ASSISTED_FIRE
            if((target_armor.getID()==serial.rxpackage.another_shoot_id)&&(target_armor.box_pos.getPosMessage().z>rm_setter.up_min_shoot_distance)) setShootflag(false);
#endif
            if((target_armor.box_pos.getPosMessage().z>rm_setter.up_max_shoot_distance)) setShootflag(false);
#endif
//            bool not_shoot_in_watch_outpost=((serial.rxpackage.outpost_alive==1)&&(abs(serial.rxpackage.chassis_speed.type_short)>50));
//            bool not_shoot_in_cruise=((serial.rxpackage.outpost_alive!=1) && (abs(serial.rxpackage.chassis_speed.type_short)<setter.not_shoot_chassis_speed));
//            if(not_shoot_in_watch_outpost||not_shoot_in_cruise)  setShootflag(false);
            txp(send_message,target_armor.getID(),target_armor.armorSize(),getShootflag());
        }
//    roi.copyTo(show_pic);
        serial.txpackage=txp;
        char info[500];
        sprintf(info, "tx=%.1f, ty=%.1f, tz=%0.1f", target_armor.box_pos.getWorldCoord().x, target_armor.box_pos.getWorldCoord().y, target_armor.box_pos.getWorldCoord().z);
        putText(show_pic, info, cv::Point(50, PICTURE_ROW_NUM - 50), cv::FONT_HERSHEY_TRIPLEX, 0.8,cv::Scalar(20, 20, 40));
        if(rm_setter.show_armors) showArmors(show_pic,_armors);
        cv::Point2f center = target_armor.center();
    }
    else LOGW("NO PIC FROM CAMERA!");
    if(rm_setter.print_state_switch_test_info&&getShootflag()==0) LOGM(STR_CTR(WORD_LIGHT_GREEN, "EMPTY POS:getshootflag:%d"),getShootflag());
};//便于进行避开串口接收的线程

void rm::ArmorFinder::solvePosNSend(cv::Mat &src){
    systime t1,t2;
    getsystime(t1);
    sendTarget( serial,serial.txpackage);
    getsystime(t2);
    LOGT("solvePosNSend:%.1fms",getTimeIntervalms(t2,t1));
};//2中pixel_predict有使用串口数据